#ifndef __RTK_DSP_H__
#define __RTK_DSP_H__


namespace rtk {

///
/// \brief One-dimensional digital filter (Direct Form II Transposed)
///
/// The calculation equation is (in Matlab style):
///     a(1)*y(n) = b(1)*x(n) + b(2)*x(n-1) + ... + b(nb+1)*x(n-nb)
///                           - a(2)*y(n-1) - ... - a(na+1)*y(n-na)
///
/// \param ord  - order
/// \param a    -
/// \param b    -
/// \param np   - number of point in 'x'
/// \param x    - input data
/// \param y    - output data
///
/// \see Matlab 'filter' function
///
template<class T>
void filter(int ord, T *a, T *b, int np, T *x, T *y)
{
    int i, j;

    // initial data
    y[0] = b[0]*x[0];
    for(i=1; i<ord+1; i++) {
        y[i] = 0.0;
        for(j=0; j<i+1; j++)
            y[i] = y[i] + b[j]*x[i-j];
        for(j=0; j<i; j++)
            y[i] = y[i] - a[j+1]*y[i-j-1];
    }

    // for the rest data
    for(i=ord+1; i<np+1; i++) {
        y[i] = 0.0;
        for(j=0; j<ord+1; j++)
            y[i] = y[i] + b[j]*x[i-j];
        for(j=0; j<ord; j++)
            y[i] = y[i] - a[j+1]*y[i-j-1];
    }
}

///
/// \brief Zero-phase forward and reverse digital IIR filtering.
///
/// \param ord  - order
/// \param a    -
/// \param b    -
/// \param np   - number of point in 'x'
/// \param x    - input data
/// \param y    - output data
///
/// \see Matlab 'filtfilt' function
///
template<class T>
void filtfilt(int ord, T *a, T *b, int np, T *x, T *y)
{
    int i;
    T   *xx;

    xx = new T[np+2];

    // forward filter
    filter(ord, a, b, np, x, xx);

    // reverse the series for filter
    for(i=0; i<np; i++) y[i] = xx[np-i-1];

    // do filter again
    filter(ord, a, b, np, y, xx);

    // reverse the data back
    for(i=0; i<np; i++) y[i] = xx[np-i-1];

    delete [] xx;
}

///
/// \brief Zero-phase forward and reverse digital IIR filtering.
///
/// \param ord  - order
/// \param a    -
/// \param b    -
/// \param np   - number of point in 'x'
/// \param x    - input/output data
///
/// \see Matlab 'filtfilt' function
///
template<class T>
void filtfilt(int ord, T *a, T *b, int np, T *x)
{
    int i;
    T   *xx;

    xx = new T[np+2];

    // forward filter
    filter(ord, a, b, np, x, xx);

    // reverse the series for filter
    for(i=0; i<np; i++) x[i] = xx[np-1-i];

    // do filter again
    filter(ord, a, b, np, x, xx);

    // reverse the data back
    for(i=0; i<np; i++) x[i]  = xx[np-1-i];

    delete [] xx;
}

} // end of namespace rtk

#endif // end of __RTK_DSP_H__

